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ABSTRACT 



The U.S. Navy and a number of its contractors are 
presently developing unmanned miniature submarines for 
several vital underwater missions. These include surveil- 
lance, submarine tracking, and bottom mapping. Foregoing 
Reserarch at NFS produced a "testbed” as a research platform 
for demonstrating the performance of AUVs. Combining the 
power of an IBM PC/AT in conjunction with a high level 
programming language, a state space dive control system was 
developed and instituted for the 3 0 inch AUV model. 
Parameter Estimation using a Recursive Least Squares Fit 
scheme and a State Observer were incorporated in the 
controller. Procedures dealing with hardware/software 
interfacing, AUV simulation analysis, and computation speed 
of large programming code were investigated. 
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I . INTRODUCTION 



A. GENERAL 

The crux of this thesis is to develop, test and evaluate 
a real-time state-space derived dive plane controller for 
the NFS prototype Autonomous Underwater Vehicle (AUV) . The 
U.S. Navy and a number of its contractors are presently 
developing unmanned miniature submarines (AUV's) for several 
important underwater missions. These include surveillance, 
submarine tracking, submarine decoys, bottom surveying, and 
off-board . sensor support for both surface ships and 
submarines [Ref. 1] . The shape of these vehicles is similar 
to that of a large torpedo and can be deployed on 
submarines, and surface ships. 

The AUV testbed at the Naval Postgraduate School (NFS) 
currently being developed is to examine the use of, and 
problems associated with, advanced control technologies. 
The objectives include: 

- determine requirements imposed by the implied need for 
artificial intelligence, 

- development of advanced computer control concepts and 
architecture , 

- determine system interface feasibility. [Ref. 2] 

Unlike the conventional body-of-revolution design, the 

NFS testbed is similar to a low aspect ratio wing. It is 
similar conceptually to the late model Swimmer Delivery 
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Vehicle (SDV) design, but actually based on a two and three 
to one aspect ratio box shape vehicle. Vehicle selection 
was based initially on the availability and thoroughness to 
which the vehicle hydrodynamic characteristics were modeled 
and validated, providing for ease of program development and 
verification [Ref. 3]. 

Modern trends in control system design are moving 
towards greater complexity. The desire for complex tasks 
and greater accuracy fuel the need for high powered 
computers to accomplish real-time control tasks in short 
duration. One of the main interests here is to determine 
the capabilities of real-time control with adaptive 
autopilots and various computer architectures. The need for 
adaptivity lies in the need to achieve robust performance 
over a wide range of operating conditions. Also, although 
computational speed may suffer, rapid response is seen to be 
important when evasive and/or obstacle avoidance maneuvers 
are called for at the supervisory/ expert system level. It 
follows that the design trade-off between response and 
robustness requires examination. For these reasons, this 
particular work has concentrated on the development of a 
real-time controller including the ability to perform 
parameter estimation, and the issues surrounding its 
response time performance. 
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B. REVIEW OF PREVIOUS WORK 



The initial proposal from the Naval Postgraduate School 
for research on an Autonomous Underwater Vehicle began 1 
October 1987. It was titled "Navigation, Path Planning, 
Dynamics and Control of Generic Autonomous Underwater 
Vehicles," and was submitted to the Naval Surface Weapons 
Center in Silver Spring, Maryland. The primary stated 
objective was to assess the capabilities for the design, 
construction, testing, and operation of a testbed as a 
research platform for demonstrating the performance of AUVs 
[Ref. 2]. 

The first task involved selecting a suitable submersible 
model. A non-conventional wing-shaped model with body cross 
sections nearly rectangular rather than circular became the 
selected choice. 

Using Dynamic Simulation Language (DSL) , Boncal (1987) 
modified the existing equations of motion (DTNSRDC 2510) and 
performed all the necessary differentials for their lineari- 
zation to suit the needs of the AUV controller development. 
The linearization was necessary to the design of model-based 
controls and to simulate controlled responses for all six 
degrees of freedom of vehicle motion [Ref. 3]. Both linear 
and non-linear models were developed and evaluated. This 
lead to the initial development of a controller robust 
enough to handle a variety of reflexive type maneuvers over 
a wide range of speeds (in the programming language DSL) . 
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In order to investigate the real-time control issues, 
and to provide a means to generate experimental data for the 
evaluation of parameter identification methods, a later 
effort by Brunner (1988), set out to design, build, and test 
a model 3 0 inches in length and 7 inches wide. The model 
was self-propelled and remotely controlled via radio 
transmission. It was equipped with rate gyro sensors, a 
pressure cell depth sensor and a pressure cell speed sensor. 
Two DC motors provided the propulsion power [Ref. 4]. Open- 
loop dive plane dynamic response tests were performed in a 
water tank where response measurements of depth were made by 
pressure cells, and pitchrate by a pitchrate gyro. Data 
from each run were recorded using PCLAB and the DT NOTEBOOK 
[Ref. 5] data acquisition software, and analyzed using the 
MATRIXx [Ref. 6] data analysis capability. Comparing these 
data with the computer model simulation, hydrodynamic 
coefficients and system transfer functions were developed. 

Initial developments of a digital autopilot to support 
the NFS test vehicle were made by Delaplane (1988) , wherein 
an IPM PC/AT was used in conjunction with DATA TRANSLATION 
Analog to Digital (A/D) interfacing boards, as the platform 
for the real-time closed loop controller. A simplified 
control program for AUV dive plane commands had been 
implemented. Signal generators were used to replicate AUV 
transducer outputs of depth, speed, and pitchrate to verify 
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system operation [Ref. 7]. The inclusion of real control 
laws, and the system parameter identification, however, was 
left to the activity reported herein. 

C. SCOPE OF WORK 

The scope of this investigation was to develop the 
digital autopilot [Ref. 7] into a three state feedback. 
Single Input multiple Output, real-time closed-loop dive 
control system for the NPS AUV model. Initial stages of 
research lead to the notion that the pitch-heave coupling 
prevalent in many underwater vehicles was not significant 
for this vehicle. The three state variables of the dynamic 
model were then reduced to the pitch rate, pitch angle, and 
the water depth. Because of tank length limitations it was 
foreseen that an analog computer model of the vehicle 
dynamics would be essential to the ease of the real time 
control development, and therefore, a scaled simulator was 
built. 

The final stages of program development were to 
implement the digital dive plane controller compatible with 
the 30 inch AUV model. Considering the small vehicle size, 
and relatively high component costs, the NPS AUV was unable 
to include a pitch angle sensor. Since pitch angle feedback 
was later shown to be critical to the autopilot stability. 



where 


the 


pitch 


angle 


could 


not be measured 


directly, a 


state 


reconstruction 


system 


was needed 


and 


had 


to be 


designed. 


This 


led to the 


development 


of a 


full 


state 
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observer driven by measured inputs and outputs [Ref. 8]. 
The Observer, including the identification of pitch rate 
sensor bias, the bias compensation, and its real time 
performance was large part of this effort. 

D . APPROACH 

Primarily, a solid understanding of the software/ 
hardware used was essential. In this control system, a high 
level programming language (Turbo Pascal) was interfaced 
with an A/D translation board (hardware) via a software 
package (PCLAB) . References 9, 10, 11 established a 
sufficient base on which the elaborate program schemes could 
be developed effectively. Once accomplished, the challenge 
of debugging large programs could readily be resolved. 

Secondly, incorporating experimental data from previous 
AUV research proved paramount. This research thesis could 
not be accomplished without the foundation set by preceding 
studies . 

Lastly, using real-time feedback of AUV model dynamics 
via the analog computer strengthened the ease by which 
program development could proceed. The Analog Computer 
simulated the model dynamics including the sensor bias and, 
partially, the effects of changing speed. This provided for 
the preliminary design of a robust state-space controller. 
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E . WHAT FOLLOWS 

In Chapter II, some detail is provided concerning the 
models of the vehicle dynamics. Chapter III discusses the 
analog computer modelling. Chapter IV gives an account of 

the program development. Chapters V and VI contain a 

discussion of results and Chapter VII is a summary and 
conclusion. 
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II . SIMPLIFIED MODELING OF VEHICLE DIVE DYNAMICS 



A. GENERAL 

In this chapter, a review of earlier experiments 
conducted under the work of Brunner [Ref. 4], appears and 
the details of the third-order model of the vehicle is 
given. This leads to a discussion of the design issues in 
the controller and the observer systems and the special 
problem of identifying the pitch rate sensor bias from input 
and output data. 

B. REVIEW OF OPEN LOOP EXPERIMENTS 

Open loop experiments were conducted during January, 
1988 (Brunner) , in which the model AUV was driven by open 
loop command to perform dive plane maneuvers. From these 
test runs, MATRIXx datafiles were constructed for both the 
command voltage to the dive plane and the measured voltages 
corresponding to the pitch rate, and the water depth 
responses. Using the linear model time history response 
features in MATRIXx, together with many simulation runs in 
which hydrodynamic parameters were varied both in value and 
in combinations of values, several "best fit" parameter 
combinations were synthesized to represent the dynamics of 
the model AUV. It was discovered that the influence of 
coupling between the heave mode, and the pitch mode dynamics 
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was not strong in determining the pitch rate behavior, and 
that for the purposes of control shape design, the heave 
motion would be neglected. The influence of the power cord 
was found to be a determining factor in the overall heave 
response, but future tests would be conducted by 
neutralizing the weight of the cord, and this effect would 
not adversely affect closed loop results. 

The numerical values of the vehicle parameters, with 
their respective dimensional units, were established as 
follows in the next section. 

C. THIRD-ORDER MODEL FOR DIVE-PLANE DYNAMICS 

The vehicle equations of motion in the dive plane are 
the surge, heave, and pitch motion equations together with 
the kinematical relationships linking the global 
longitudinal and vertical velocity components to the body 
fixed surge, heave, and pitch rates. These are given in 
detail in Boncal (1987). When linearized about a constant 
speed straight line flight path, the relationships simplify 
to four first-order state equations in the heave and pitch 
rates and positions. In short, 

ZW*UX*W ZQ*UX*q ZD*UX^*6 

^ MZDW MZDW L*MZCW MZCW 

iyW*UX*W ^ MQ*UX*q MD*UX^*6 F2 

L*IYY ^2*^ lYY 

0 = q 
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z = w - ux*e 



where w, q/ 6 , z are the four state variables and the 
parameters are given by, 



HEAVE DAMPING 


ZW 




-1.5 


PITCH CROSS COUPLING 


ZQ 


= 


0.0 


HEAVE CROSS COUPLING 


MW 


= 


O 

• 

o 


PITCH DAMPING 


MQ 


= 


-0.15 


MOMENT EFFECT 


ZD 


= 


0.0 


MOMENT EFFECT 


MD 


= 


0.225 


MASS PLUS ADDED MASS 


MZDW 


= 


1.005 


INERTIA PLUS ADDED MASS 


lYY 


= 


0.072 


EFFECT OF TETHER 


FI 


= 


0.11 


EFFECT OF TETHER 


F2 


= 


O 

• 

o 


FORWARD SPEED 


UX 


= 


2.1 FEET/SEC 


LENGTH 


L 


= 


2.5 FEET, 



where the units of the parameters are given in Table 2.1. 



TABLE 2.1 
PARAMETER UNITS 



Parameter 



MQ*UX/L*IYY 

UX 

MD*UX2/l2iyY 



Units 

sec“^ 

ft/sec 

rad/sec^ 



Value 

(Dimensional) 

10.0 1/sec 

10.0 ft/sec 

10.0 l/sec2 



Value 

(Volts Units) 
1.8 
2 . 1 
2.531 
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It should be pointed out that the matching of experimental 
data was done by comparison of sensor output voltage 
response time history and that proper conversion to 
dimensional values was essential to being able to translate 
to control gains for real time control system design. 

Because it was found that the effects of the heave/pitch 
coupling were not strong, and that the influence of w was 
dominated by the power cord, it was assumed that for the 
purpose of control system design, the heave dynamics would 
remain unmodelled, and the model base would be reduced to 
third-order with the equations as follows; 

X' = [q,0 ,Z] , 



-1.8 


0.0 


0.0 




2 . 531 


1.0 


0.0 


0.0 


X + 


0.0 


0.0 


-2.1 


0.0 




0.0 



D. FULL STATE FEEDBACK CONTROL DESIGN 

For the nominal forward speed of 2.1 ft/sec, the system 
dynamics matrix, and the system input and output matrices 
are; 





-1.8 


O 

• 

o 


1 o 

• 

o 




2.531 


A = 


1.0 


o 

• 

o 


o 

• 

o 


B = 


0.0 




0.0 


-2 . 1 


0.0 




0.0 
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These equations are characterized by the open loop 



eigenvalues : 



V = [- 1 . 8 , 0 . 0 , 0.03 



which properly indicates the first-order pole for the pitch 
rate response and the double integration from pitch rate to 
depth. The output matrix has diagonal coefficients with 
either unity values or other constants depending on whether 



the output 


signals are considered 


to 


be 


sensor 


output 


voltages or 


the dimensional values 


of 


the 


system 


state 



variables . 

Noting that the open loop system is completely 
controllable in the formal sense, a pole placement technique 
was determined to be appropriate for the autopilot design. 
Additionally, since the overall system required a digital 
controller, discrete time methods were needed, and the 
question of sample rate arose. 

Digital systems for the control of continuous processes 
are limited to providing a sequence of stepwise constant 
levels that can be updated every sample time, but remain 
fixed within that interval. Clearly, a short interval is 
preferred. However, the execution time of extensive amounts 
of code requires that the sampling period must be sufficient 
for code execution, but it must not be so short as to 
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places limitations on the closed loop poles as specified for 
the control design, being consistent with the execution time 
of the real time code. 

In balancing the foregoing, two systems were designed. 
The first, at 2 0 Hz, was designed to accommodate a control 
code that executed a full three state feedback design where 
the system parameter identification was bypassed. The 
second was at 5 Hz (the maximum upper limit possible under 
the present hardware configuration) where parameter 
identification was performed. 

The open loop system dynamics matrix and input matrix 
were then discretized using the 20 Hz sample rate so that 
the new A, and B matrices became: 





.9139 


0.0 


0.0 




. 121 


A = 


. 0478 


1.0 


0.0 


B = 


. 0031 




-.0025 


-.1050 


1.0 




-.0001 



Closed loop poles for the 20 Hz system were then specified 
at 



s = [0.92,0.925,0.926] 

in the z-domain, corresponding to three poles in the s-plane 
with time constants equal to 1.0 sec, with the following 
state feedback gains. 
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Kc*X = [0.3066,0.8604,-0.2493], 



where allowances were made for the output transducer gain. 
For a state regulator system, the control law is, 

u = -Kc*x 

when constant non-zero command signals are to be introduced, 
the control law requires modification to, 

u = N*r - Kc*x 

in which the command (s) r and the set point scaling matrix, 
N, are specified so that the tracking error r-y vanishes. 
The closed loop system equations become 

X = (A-B*Kc) *x+B*N*r; y = C*x 

and the tracking error vanishes in steady state when, 

C* (A-B*Kc) “1^‘B*N=I; N=(C* (A-B*Kc) “^*B) 

yielding the discrete time control law used, 
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u(k) = -0.2493*[r(k)-z(k) ]-z(k) ] 

-0. 8604* [e (k) ]-0. 3066* [q(k) ] 

This control provides reasonable response time and calls for 
a control surface activity level that would exceed the 
stroke of the vehicle dive plane. 

E. OBSERVATION OF UNMEASURED STATES 

In the development of the model AUV the size limitation 
prevented the inclusion of an angle sensor for pitch, so the 
control system was required to reconstruct that signal in 
order to yield stable results. The application of a full 
state observer, configured as a model based compensator is 
given by the following one step prediction equations, 

^ /N ^ 

x(k+l) = A*x(k) + B*u(k) + Ko* [y (k) -C*x(k) ] 

in which y(k) is the previous time sensor output signals. 
In this case, two outputs were available to the observer, 
the pitch rate, and the water depth. The observer gains, 
Ko, may be selected by using the dual of the controller pole 
placement technique, or by using Linear Quadratic Regulator 
optimization. Although pole placement techniques in general 
are based on a single input signal, two inputs can be 
handled by the assignment of some weighting value (i.e., 
equal) ineffective towards the outcome. The effect of the 
two signals, y(k) , now becomes. 
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y(k) = [l;l]*ye(k) 



so that the gains for the two-input system become 

Ko = [l;l]*ke 

where Ke are found via pole placement using ye(k). 

The inclusion of the observation (and later parameter 
identification) algorithms required the use of a slower 
sample rate, 5 Hz. Consequently, enclosed loop poles of the 
observer were chosen as a compromise between fast response 
and the resulting sensitivity to extraneous noise when 
implemented as a model based compensator. The following 
were the final selection, 

Ko = [0.0, *-0.4029, *0.5677] 

F. PARAMETER ESTIMATION OF SENSOR BIAS 

Parameter identification of the rate gyro bias was 
accomplished by the use of an ARMA model of the gyro system 
itself where the input variable was the dive plane angle and 
the output was pitch rate. The ideal transfer function 
between input and output is a first order system which has 
two numerator coefficients, and two denominator coefficients 
to identify. (Note that the z-domain transfer function is 
to be used because we are dealing with sequences of sampled 
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data.) With this form, the parameter vector and the data 
vector can be written as, 

e = [1 al bO bl] ; x' = [y(t) y(t-l) d(t) d(t-l)]; 



and 



6*x = bias (t) . 

The foregoing is then repeated and a least squares fit 
for 6 is then obtained after the bias is included inside the 
vector 6. Least squares solution of these equations yields 
both the vector of parameters, 6 , and the unknown bias. In 
this work, a recursive least squares algorithm was used with 
a Householder routine to compute the recursive gains which 
is time consuming but robust. 
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Ill . ANALOG SIMULATOR 



A. GENERAL 

Simulation, a must for prototype development, provides a 
source of numeric, kinematic and logical awareness which is 
vital in designing a complex vehicle system and analyzing 
its performance. It provides an environment in which the 
many possible states of the system, and the transitions 
between them, can be exercised and observed in an accessible 
and controlled manner. Properly utilized, simulation will 
reduce significantly the amount of field testing required. 
Design and performance deficiencies can be identified early 
in the development cycle, before the system is deployed 
[Ref. 12]. 

A technique known as "Hybrid" simulation employs a 
combination of analog and digital computing [Ref. 13]. 
Using the hydrodynamic coefficients of the AUV model, the 
analog computer can be "programmed" to simulate AUV vehicle 
dynamics. With the control program running and a "target 
depth" entered as a keyboard input, AUV pitch angle, depth, 
and pitchrate, calculated by the analog computer, are fed 
back to the digital computer. The three states are compared 
to a model reference and an appropriate dive plane command 
is determined. The result is a continuous update (real-time 
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control) of vehicle dive plane angle (see Figure 3.1 for 
hardware configuration) . 

In the second phase of program development, the analog 
computer was configured to sustain a constant or variable 
bias on the system. This lead to the development of a bias 
estimator . 

B. DESCRIPTION OF ANALOG SIMULATOR 

In developing the analog simulations, the equations of 
motion of the vehicle are required to be converted. 
Convenient conversion or scaling is based on the maximum 
variable values being scaled with a +/- 10 volts maximum 
output on each amplifier. In analog simulations, variable 
summations and integrations are performed by summing and 
integrating operational amplifiers. Multiplication by 
constant coefficients are performed by attenuating potenti- 
ometers set to the appropriate fraction of 1.0. With 
scaling consistently applied through the 1 or 10:1 input, 
appropriate gains are sent to the corresponding amplifiers. 

Figure 3.2 shows an example of a first order system 
simulation using an analog operational amplifier. The 
equation is: 



q = [1.5] 6 rad/sec 
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PITCH ANGLE 0 



Figure 3.1 Hardware Configuration 
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DELTA 




Figure 3.2 Example of a First Order System 

Using an Analog Operational Amplifier 



where 6 is in radians. Note the coefficient 1.5 rad/sec per 
radian is set at 0.15 in the potentiometer with a gain of 10 
in the integrating amplifier. 

In Figure 3.3, the diagram represents the third order 
model of the vehicle given by Equations in II. C. Table 2.1 
gives a listing of the system parameter values, their 
dimensions, and the potentiometer settings used in the 
simulations . 
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Figure 3.3 Vehicle Simulation Analysis 
Using an Analog Computer 
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IV, DIGITAL CONTROL PROGRAM DEVELOPMENT 



A. GENERAL 

As explained in Reference 1 , this digital control 
program represents a functional component at the lowest 
level of the digital autopilot echelon. The digital control 
program interfaces with the servos controlling the AUV dive 
planes. The "handshaking" of data between the digital 
controller and the voltage-operated servos onboard the AUV 
model are accomplished by A/D interface hardware [Ref. 14]. 
In Figure 3.1 the hardware arrangement using the analog 
computer as a simulator of sensor feedback is exhibited. 

Much of the program development of Reference 7 is 
implemented into this thesis. A number of dilemmas had to 
be overcome for program progression to continue. 

The preliminary program code had parameter conversions 
of feet/sec, inches, and degrees/sec. To alleviate valuable 
computation time, and to better support the AUV model, the 
code for the input and ouput (I/O) dimensions was modified. 
All I/O parameter dimensions were converted to voltages. 

Significant concern focused on the programmable inter- 
rupt control . Previous work was hampered when software 
interaction was unachievable. The preliminary control 
program would run only when initial keyboard inputs of 
target depth were entered. Pre-programmed keyboard 
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interrupt inodes would not respond to subsequent inputs. 
Keyboard interaction was masked and additional dive plane 
commands could not be executed. The program could terminate 
only if the CTRL- BREAK key combination was executed. 

From Reference 11 it was discovered that by removing the 
influence of the CTRL-BREAK character at the start of the 
programming code, programmed interrupts could be invoked. 

By overcoming this impasse, program development was 
inevitable. Significant algorithm progression followed. 
"Canned" maneuvers, invoked by keyboard inputs, were 
developed and tested. Data collection and filing were 
implemented by actuating function key inputs. 

The final phase of research was committed to the 
development and implementation of an Estimator/Observer 
control program. This algorithm would reconstruct the state 
variable, pitch angle, using depth and pitchrate sensor 
feedback. Bias created by the relatively inexpensive 
onboard sensors was determined by a Kalman filter and 
removed from the system. A cleaner, more robust control 
signal resulted. 

B. A/D, D/A CONVERSION 

The Analog to Digital (A/D) and Digital to Analog (D/A) 
conversion process is vital to the operation of this control 
system. Analog sensor signals from the AUV are digitized 
via the A/D process prior to entering the computer control 
program. Likewise, digitized command signals are converted 
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to analog signals via the D/A process prior to return to the 
dive plane servos. For the preliminary AUV autopilot design 
described in Reference 7, the total cycle time for a 
complete I/O conversion process was 5 milliseconds with a 50 
millisecond sample rate (20 Hz) . In later paragraphs, 
discussion will focus on the need to decrease the sample 
rate in order to augment the larger, more complex control 
code . 

The digitizing process is accomplished by the Data 
Translation (DT) board, DT 2801-A. It is installed internal 
to the computer as explained in Reference 7. The board 
configuration can be found in the technical reference manual 
[Ref. 14]. All conversions on the DT 2801-A board are 
controlled by writing data to, and reading data from, 
registers on the board. PCLAB subroutines is an interfacing 
software package that coordinates this task [Ref. 15] 

Conversion of a voltage value to a digitized value 
depends on the number of bits of resolution used by the 
converter. For instance, a 12-bit converter has 4096 Number 
Of Codes (NOC) . For a bipolar range of +/- 10 volts, a 0 
NOC would correspond to -10 volts, while a 4096 NOC would 
correspond to +10 volts. An NOC of 2048, similarly, would 
correspond to 0.0 volts. 

Before these digitized voltage values are usable in the 
software control program, they need to be scaled to an 
equivalent numerical voltage value. Equations found in 
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Appendix D of [Ref. 15] were implemented into the control 
code for such a reason. 

C. REAL-TIME CONTROL CODE 

The control program code was written in the programming 
language Turbo Pascal version 3.0. A more proficient 
version (V4.0) is presently available, but it is not 
currently supported by the interfacing software (PCLAB) . 

A real-time controller is "real-time" up to the limits 
of the software program implementing it. A significant 
advantage of Turbo Pascal is operation speed. Reducing 
large problems down into smaller, easier defined procedures 
affords faster execution. Each procedure can then be 
subdivided further, at the discretion of the programmer. 

The control program executes an AUV dive control system 
within a user friendly multiple menu-shell setup. When 
running the compiled control program, the first screen 
encountered is the Main Menu. Here, the user is presented 
with a display of program titles and the options to quit or 
continue (run) . The next screen offers a similar choice, 
where depressing the function key FI starts the control 
mode. In this mode, the user is asked to name a "data file" 
(standard IBM DOS file) . The results of the vehicle 
dynamics during the course of the run will be stored in this 
file. The final input screen prompts the user to "ENTER THE 
TARGET OPERATING DEPTH." Completion of this procedure 
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activates the closed-loop dive plane control. The Run-Mode- 
Screen is introduced on the monitor displaying updated 
values of AUV depth, pitch, pitchrate, dive command angle, 
sensor bias, and attitude. 

During closed-loop control, the user has four methods of 
operation: 

- F3 — exit from the "Active Control Mode," and file close- 
out . 

- F2 — activates a pre-programmed "Canned" maneuver. 

- FI — exit from the "Active Control Mode," close-out 
previously selected file, creates new file with user 
interaction, and requests new target depth to reactivate 
closed-loop control. 

- Any alpha/numeric key — temporarily exits from "Active 
Control Mode," prompts user for a new target depth, 
while retaining previously selected data file for data 
storage . 

Two separate control programs were used to establish 
data collection and analysis. The first, AUVC0P3F. PAS , was 
designed as a closed-loop dive control system with full 
state feedback. The "C0P3" corresponds to the third copy or 
generation control program. The "F" coincides with data 
Filing capabilities. "PAS" is Turbo Pascal's standard file 
type designation for programs to be complied. The second 
program, AUVC0P4F. PAS , implements an Observer/Estimator to 
reconstruct an unknown state variable from two sensor 
inputs. Sensor Bias is also estimated and removed from the 
system. 
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D. SAMPLE RATE CONTROL 

A signal generator excites the DT 2801-A board to pass 
data to and from the data registers. The dial setting of 
the generator determines the rate at which this is 
accomplished. As mentioned earlier, a sample rate of 20 Hz 
was used in Reference 7. 

When running the full-rate feedback control program 
(AUVCOP3F. PAS) at 20 Hz sample rate. Turbo Pascal computed 
the relatively small algorithm in 5 milliseconds. Manually 
reducing the sample rate during program runtime resulted in 
a stable controller as low as 3 Hz. An ideal condition, 
though, is to maintain a high sample rate to yield a more 
stable, faster responding controller. 

The Estimator/Observer control program (AUVC0P4F. PAS) , 
however, contains an additional 400 hundred lines of code. 
Here, computation time was increased significantly (greater 
than 150 milliseconds) and could not keep pace with the 20 
Hz sample rate. To accommodate, the sample rate was reduced 
to 5 Hz. Likewise, new gains were derived. 

Current hardware/software configurations of the AUV 
controller are limited in performing these multiple matrix 
calculations at high sample rates. 

E. SOFTWARE LIMIT FOR ACTUATOR SATURATION 

Present hardware configurations on the 30 inch model 
required small input voltages to actuate the full range of 
the dive plane angle. During previous research of open-loop 
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tests, dive plane angle was compared with the corresponding 
input voltage. Results concluded that a linear 
proportionality on the order of 0.01 volts per degree dive 
plane angle existed. This was true for a range of 0.0 to 
10.0 degrees. Hence, the operating range of the dive planes 
became +/- 10 degrees, and respectively, the operating 
voltage range became +/- 0.1 (using the hand-held 
transmitter). The dive plane voltage range of +/- 0.1 was 
implemented into the analog computer configuration. The 
control code was changed by creating a software stop prior 
to sending the final command signal to the plane. This was 
implemented by limiting the final signal sent through the 
D/A conversion to +/- 0.1 volts in the form, 

if abs(u) > 0.1 then u = 0. l*abs (u)/u. 

F. FILES FOR DATA RECOVERY 

During the open-loop vehicle tests, data acquisition was 
achieved by using the software program DT Notebook [Ref. 5] 
on the IBM PC/AT. The computer's system configuration file 
had to be specifically structured for DT Notebook operation. 
However, the programming language Turbo Pascal used an 
entirely different configuration. The two programs, 
therefore, could not be run simultaneously. In other words, 
data acquisition by DT Notebook could not be performed while 
running the closed-loop control program. 
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An alternate method of data recovery was devised using 
Turbo Pascal [Ref. 11]. The keyboard commands for data file 
use are found in Chapter IV. C. Although additional through- 
put is needed for data file simulation, the time used in 
writing data to a hard disk was found to be negligible in 
comparison to the sample rate. 
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V. CLOSED-LOOP RESULTS 



A. GENERAL 

In Figure 5.1, the closed-loop full state feedback 
system is displayed in block diagram convention. It is 
comprised of the system model: 





Figure 5.1 Full State Feedback Diagram 



X = [A]x + [B]u 



and the feedback control: 



u = [N]r - [Kc]x 
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where the input of target depth is the set point r, the 
closed loop gains are and the setpoint matrix N is 
designed for zero steady state depth error. The following 
sections in this chapter evaluate the response of the above 
system under varying conditions. The gains are fixed and 
the sample rate is 20 Hz. 

B. RESULTS OF FULL STATE FEEDBACK 

In the full state feedback controller, the closed loop 
gains are designed for an AUV speed of 2.1 Ft/s. In the 
following figures, AUV depth voltages ranging from 0.0 to 
10.0 volts represent the water depth from the surface to a 
depth of 10.0 feet below the surface respectively. The dive 
command voltage ranges from'+/- 0.1 volts equivalent to a 
dive plane angle of +/- 10 degrees. In Figure 5.2, a target 
depth of 6.0 volts becomes the new set point from 1.0 volt. 
Subsequently, a dive command of 10 degrees is sent to the 
dive planes. Almost immediately, the AUV responds and 
levels out at the new depth. The entire "run" lasts 
approximately 120 sample increments or six seconds, as 
expected, with an overall dominant time constant close to 
the 1.0 second used in the gain design process. 

C. EFFECT OF ACTUATOR SATURATION 

As previously mentioned, the dive plane actuator is 
limited to a range of +/- 10 degrees. If this limit is 
removed then the results of Figure 5.3 are found. Comparing 
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150.0 175.0 200.0 225.0 250.0 275.0 

TIME(INCREMENTS OF .05 SEC) 

Figure 5.2 Depth Maneuver from 1 to 6 Feet at a 
Speed of 2 Ft/s and 20 Hz Sample Rate 
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this to a second run, having actuator saturation, the 
results of Figure 5.4 show that the non-saturated dive 
command achieves a slightly faster AUV depth response. 
Figure 5.4 attains 5.0 volts in approximately six seconds 
whereas Figure 5.3 attains target depth in approximately 5.5 
seconds. In Figures 5.5 and 5.6, a similar comparison is 
made. Here, however, the depth change is considerably 
smaller. Similarly, the response time difference is less 
noticeable . 

As one would expect, a larger actuator in the dive plane 
drives a quicker AUV response. While actuator rate-of- 
change limits may be an additional limit to consider, these 
results assumed that such a limit was not present. 

D. EFFECT OF VEHICLE SPEED 

Though the closed loop gains were designed to operate 
the AUV at a speed of 2 FT/s, other speeds were tested to 
determine the robustness of the controller so obtained. In 
Figures 5.7, 5.8, and 5.9, the vehicle speeds of two, four, 
and six FT/s respectively were evaluated when the AUV depth 
changed from six to four volts. In Figure 5.7, the AUV 
smoothly reaches the assigned depth without overshoot in 
approximately five seconds. In Figure 5.8, target depth is 
attained in nearly 3.5 seconds with a slight overshoot 
before leveling out. Finally, in Figure 5.9, overshoot is 
most noticeable, but target depth is accomplished in close 
to three seconds. Based on these observations, it can be 
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noted that through a 300% change in the speed parameter, the 
closed loop control performance remains stable with only a 
40% change in response time and, for overshoot, a change of 
less than 15%. 
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VI. BIAS ESTIMATION AND RESULTS 



A. GENERAL 

In this chapter, the final stages of program design are 
implemented. The results are graphed and evaluated. Here, 
the fullstate feedback controller in Chapter V is combined 
with a parameter estimator and state observer system as 
shown in Figure 6.1. The appendix details this program 
code . 

The parameter estimator determines sensor bias of the 
depth pressure cell and pitchrate gyro. The three-state 
observer determines the missing state, pitch angle, and 
filters the return signal of the remaining two states. 

With the inclusion of the above algorithms (over 400 
lines of additional code), the 20 Hz sample rate or 0.05 
seconds per sample, had to be reduced. Significant increase 
in computation time prevented a sample rate much higher than 
5 Hz or 0.2 seconds per sample. Hence, the new sample rate 
became 5 Hz and the poles in the z-plane were changed 
accordingly. The remaining figures in this chapter show the 
results of the new code at work. For purposes of 
evaluation, vehicle depth change remains constant while 
other parameters are varied. 
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Diagram of the Bias Compensated Model Based Controller 



Figure 6.1 Diagram of the Bias Compensated 
Model Based Controller 
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B. EFFECT OF SPEED CHANGE ON AUV RESPONSE 



In Figure 6.2 at an AUV speed of 2 ft/sec, a smooth dive 
maneuver from 1.0 volt to 5.0 volts yields a response time 
of 13 seconds, as compared to six seconds in Figure 5.4. 
The decrease in sample rate has moved the poles, retarding 
AUV response time by more than a factor of two. 

In Figure 6.3, the vehicle speed is doubled to four 
ft/sec and the same maneuver is invoked. Here the depth 
response curve is not as smooth or direct, but no overshoot 
is observed. Since the poles are designed for a slower 
speed, the vehicle tries to settle out prematurely, delaying 
the depth change process. This maneuver was accomplished in 
17 seconds. 

Results of this vehicle speed increased to six ft/sec 
are shown in Figure 6.4. Again, the curve is not smooth, 
with abrupt settling occurring after two-thirds of the depth 
change and then continuing on to 5.0 volts depth. The 
completion time for this maneuver is greater than 20 
seconds . 

For each increase in speed, maneuver completion time is 
extended, but target depth overshoot, however, does not 
occur. 

C. AUV RESPONSE TO A BIAS SIGNAL 

As mentioned in preceding chapters, the return signals 
from the relatively inexpensive sensors onboard the 30 inch 
model are prone to include sensor bias. In the subsequent 
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figures, the results of vehicle depth with sensor bias is 
shown. A 0.1 volt sensor bias in Figure 6.5 results in a 
corresponding 0.1 volt decrease in actual depth as compared 
to the target depth of five volts. Although maneuver 
completion time is not influenced (equivalent to Figure 6.2 
with zero bias) , the target depth is never quite reached. 
Similarly, in Figure 6.6, a larger sensor bias of 0.2 volts 
results in a vehicle depth falling short of the target depth 
by the same amount. 

By slightly adjusting the program code, the estimated 
bias can be removed from the system resulting in a cleansed 
depth response signal, shown in Figures 6.7 and 6.8. 
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VII . SUMMARY AND CONCLUSION 



A . SUMMARY 

This thesis presents the implementation of a real time 
autopilot dive plane controller for the NFS AUV testbed. 
The controller includes a state observer and sensor bias 
estimator. The approach to the design of the autopilot 
includes : 

- Manipulation of vehicle dive dynamics to develop 
linearized equations of motion, 

- Implementation of an Analog Simulator using data from 
open loop control. This affords more effective and 
efficient development of the full state and partial 
state feedback control algorithms, 

- Hardware/Software interface via A/D and D/A conversion 
boards , 

- Program development using Turbo Pascal version 3.0, 

- Sample rate control on program code of differing 
magnitudes , 

- Incorporating data recovery via file opening techniques. 

B. CONCLUSION 

This study developed a dive plane autopilot for the NPS 
testbed that showed signs of robustness. Under closed loop 
control, the vehicle stabilized at various depths under wide 
ranges of speed with no overshoot. 

As program code increased in size, throughput increased 
reducing sample rate, and therefore, reducing autopilot 
response time. Future research of the steering control 



54 



autopilot may need to optimize current algorithms to prevent 
further throughput increase and to optimize robustness 
characteristics. Parallel-tasking processors may be an 
option to large, complex, programming code, creating a 
faster throughput. 
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APPENDIX 



CONTROL CODE FOR AUTOPILOT 



($c-) 

program AuvAutoPilot ( input, output }; 

{ TITIiE : Autonomous Underwater Vehicle Auto Pilot Program. 

AUTHOR : LT Jerry J. Relna USN 

APPLICATION : real-time controller with a Model Based Compensator to 

estimate pitch angle, and a parameter estimator to determine 
sensor bias 

DATE : 8 Dec 1988 

Project Description : This program implements digital control of the NPS 
autonomous underwater vehicle (AUV) in the vertical or dive plane.lt samples 
vehicle sensor input from two channels : depth and pitchrate. Tlie Model 
Base Compesator determines pitch angle. All states are then multiplied by 
the controller gains and compared to the target depth. An appropriate dive 
command is determined and sent back to the vehicle via D/A conversion boards. 
A deflection on the dive planes result. The sample rate is 5 Hz.) 



{ GLOBAL DECLARATIONS 



) 



const 

{ Screen declarations 

xl = 5; 

yl = 2; 

x2 = 75; 

y2 = 24; 

typ <2 

str 10 = str ing 110); 
str60 = string (60); 



var 



hr ,hr2,min,min2. 




sec,sec2,hun,hun2 


: byte 


seconds 


: real 


option, controlmode. 




reply, r eply2 


: char 



) 

{ Upper left corner : left edge ) 

I Upper left corner : upper edge ) 

( Lover right corner : right edge ) 

{ Lover right corner : bottom edge ) 



{ 



INCLUDED FILES Declarations 



) 



{$! pcldefs.tp ) { PC I.AB Trubo Pascal routines. 

{$I pclerrs.pas ) { PC LAB error code messages file. 



{$Idravbox2.auv) 

( Input xl,yl,x2,y2 : integer to specify the corner limits of the box. 
This procedure clears screen and draws a rectangular box of specified 
dimension using ASCII double line characters. 



{$Iclrbox2 .auv) 



) 

) 



1 



56 



{ Input xl,yl,x2,y2 : integer to specify the corner limits of the box. 

This procedure uses a FAST means of clearing a box of specified dimension. 



The box dimension should be delcared as constants. ) 

{ $ I boxpr i n t . au V } 

{ Input the printrov, leftboxedge, rightboxedge : integer and printstring : 

strGO. This procedure centerprints the string in the box at the printrow 
specified without overwriting the box border. ) 

{ $ I show f as t . au V } ; 

{ Input message : str60, column, row : integer. To specify the x,y position 

on the screen for a FAST message print. ) 

( $Ikeyhit .auv) 

{ This is a boolean function which returns true or false if key is pressed; 

it also returns keycode replies VAR reply, reply2 : char . } 

{ $I tabxy . auv) 

{ Input tabcol, tabr ow : integer; like gotoxy } 

[ $Iboxpause .auv) 

{ Input xpause , ypause : integer to specify where ’’Press any key to continue" 

message is to be printed. ) 

{ $I ge tkey . auv ) 

{ Input as a string of chars, the set of acceptable replies; ie *YyNn’. This 
procedure waits until one of the acceptable replies has been entered. ) 



{ ***************** main programs procedures 

{ ***************** usEB INTERFACE MODULES 
procedure MainMenu ( var reply : char ); 

{ This procedure presents the AUV screen and solicits an option to Run 

the AUV from the StatusAndCommand procedure or to Quit. ) 

beg I n 

repeat 
c 1 r s c r ; 

drnwbox2(xl /yl,x2,y2); 

boxpr int(yl+3,xl,x2, *N AVAL POSTGRADUATE SCHOO L*); 
boxpr Int(yl45,xl,x2, *D EPARTMENT OF*); 

boxpr int(yl>6,xl,x2, *M ECHANICAL ENGINE ERIN G*); 

boxpr int(yl+8,xl,x2, ’AUTONOMOUS UNDERWATER VEHICLE’ ) ; 
boxpr int(yl H0,xl,x2, ’DIGITAL AUTOPILOT CONTROL PROGRAM’ ) ; 

boxpr 1 nt ( yl + 1 5 , xl , x2 , * Do You want to RUN this program ..’); 

boxpr I n t ( yl 1 6 , xl , x2 , ’ o r Do You want to QUIT and return to DOS ?’); 

boxprint(yH20,xl,x2, ’>>>> ENTER Q OR R <<<<’); 

getkey ( ’QqRr ’, reply, reply2 ) ; 

until ( reply in t ’ Q ’ , ’ q ’ , ’ R ’ , ’ r ’ ) ) and (reply2 = chr(O)); 
end ; 
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procedure Sta tusAndCommand ( var mode : char ); 

{ This procedure begins the control program screen. 



) 



var 

mode2 : char; 

procedure StatusAndCommandScreen; 

{ This is the status and control screen and solicits a user Input of FI to 
RUN the program or Q to Quit and exit to the main menu. } 

begin { S ta tus AndCommandScr een } 

cl rbox2 (xl,yl,x2,y2) ; 

boxpr int(yl >l,xl,x2, *AUV STATUS / COMMAND AND CONTROL SCREEN’); 
boxprlnt(yl+2,xl,x2, ’= = = = = == = = = = = = = = = = = = = = = = = = = = ?= = .-= = = = = = = = = = = = = = = = = = = = =’ ) ; 
boxprlnt(yl+7,xl,x2, ’CHOOSE YOUR DESIRED CONTROL MODE 
boxprlnt(yl+9,xl,x2, ’ENTER KEY << FI >> TO START AUV CONTROL’); 
boxprlnt(yl+ll,xl,x2, ’ENTER << Q >> TO QUIT AND RETURN TO MAIN MENU’); 
boxprlnt(yl+16,xl,x2, ’PRESS EITHER FI OR Q’); 

end; { Sta tus AndCommandScr een ) 

{ ** ft * <^: ** <^ *** ^(: ******** ft * CLOSED LOOP CONTROL ROUTINES ***** ^^ ******* ^ ***** * ) 



procedure Close dLoopControl; 

{ This module comprises the closed loop control scheme, 
label 5; 

LABEL 1; 
const 

maxdepth = 33; 

mlndepth = 0; 

update increment = 10; 



type 

tr y = array [ 1 . . 10 ) 
act I vecontr o Imode 
a 1 lovabledepthr ange 



of real; 

= ( run, reset, exit ); 
= mindepth .. maxdepth ; 



auvatt I tude 



climb, maintain, diving ); 



digl tal integerar ray = array tl..3) of integer; 



) 



var 

filename rstrlng 114); 
f 1 levar : text ; 



auvdepth, auvdepthvolts , auvspeed, auvpl tch,auvpltchvolts , auvpl tchvol tsxx , 
a u vs peed VO Its , auvpl tchrate, auvpltchratevolts,estdepth,err, 

depth err or VO Its, tar getdepth volts ,blasl,divevolts,tgtnev,targetdepth,speed:r 



ea 1 ; 
adv 

j , status , t Ime 
modereply,modereply2 
act I vemode 

updatecounter, Initial 
depthr ange 



dlgitallntegerarray; 
Integer ; 
char ; 

act I vecontr o Imode ; 
Integer ; 

al lovabledepthr ange ; 
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at t: Itude 
acl 
nc2 
ac3 



procedure GetTar ge t Depth (var tgtdepth : real ; 

var tgtdepth VO 1 ts : real 

( This procedure solicits the target AUV operating 
an AUV equivalent targetdepth analog voltage and 
par ame te r s . ) 



begin ( Ge tTargetDepth ) 

clrbox2 (xl,yl,x2,y2); 

boxpr int(ylU0,xl,x2, ’FNTER THE AUV TARGET OPERATING DEPTH*); 
boxpr Int (ylf 11, xl, x2, • NOTE : THE DEPTH SHOULD BE IN VOLTS (0.0 - 10.0)'); 
repeat 
begin 

boxprlnt ( yl4 1 3 , xl , x2 , ' ENTER THE TARGET OPERATING DEPTH '); 
gotoxy (xlf33,yl4l5); 
r ead ( tgtdepth ) ; 
end ; 

until tgtdepth >= 0.0 ; 
tgtdepthvol ts := tgtdepth ; 

end; { GetTar getDepth } 

procedure RunModeScr ee n ; 

{ This procedure displays the Closed Loop Control Screen In the RUN MODE. ) 

begin ( RunModeScr een ) 

clrbox2 (xl,yl,x2,y2); 

boxpr lnt(yl+l,xl,x2, 'A UV STATUS / CONTROL SCREE N') 

/ 

boxprint(yI+2,xl,x2, *= = ==- = = = = = ===- = = = = = = = = = = = = = = = = = = =• = := = = = = = = - = = = = = = = -* ); 
boxpr lnt(yl+4,xl,x2, 'STATUS OF A U V OPERATING PARAMETERS :*); 



write 


(tabxy 


(xl+5, yl+6 ) , 


' AUV 


DEPTH 


1 volts ) 


: ’ 


wr 1 te 


( tabxy 


(xl+5, yl+7 ) , 


'AUV 


PITCH 


(volts 1 




write 


( tabxy 


(xl45,yl48) , 


’ AUV 


PITCHRATE 


(volts ) 


: ’ 


wr 1 te 
w r 1 1 e 


( tabxy 
( tabxy 


(xl45, yl49 ) , 
(xl45,yl4l0) 


' AUVDIVEPLANE 
, ' BI AS = 


(volts) 
: ’ ); 


•• ' ) ; 



boxpr Int(yl4ll,xl,x2, ' A U V CONTROL STATUS :*); 

write (tabxy ( xl4 5, yl+13 ),' CURRENT TARGET DEPTH : '); 
write (tabxy ( xl 4 5, yl4 1 4 ) , * CURRENT MODE : '); 



: auvattltude; 
: real ; 

: real ; 

: real ; 



); 

depth and converts It to 
passes both o£ tJiese 
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write (tabxy ( xl +5, yH 1 5 ) , ' CURRENT MANEUVER 



* ); 



boxpr int ( yl4 18, xl , x2. 



TREi: 



KEY 



FI 



TO ENTER NEW TARGET DEPTH. 



* ); 



boxprlnt(yl^l9,xl,x2, 

’PRESS KEY F2 .. TO STOP ACTIVE CONTROL AND RESET.’); 



boxpr int(yl^ 20,xl,x2, 
’PRESS KEY F3 . 
end; { 



TO EXIT ACTIVE CONTROL. 
RunModeScreen-^ 



* ); 



procedure UpdateRunMode Screen (updatedepth,updatepltch,updatepltchrate 
, updatedive volt : real; 

update tar getdepth : real; 
updatemode: act 1 vecontr o 1 mode ; 
upda tea t t i tude : auvattitude; 
var bias 1 : real); 

( This module updates the Closed Loop Control Run Mode Screen 
display parameters. Updates occur In Intervals specified by 
interval declared in Cl osedLoopCon tr o 1 procedure. 



with updated 
update Increment 



} 



begin 



{ 



UpdateRunModeScr een 



} 



wr i teln 
wr i te 1 n 
vr I teln 
wr i teln 
UPDATES 



( tabxy 
( tabxy 
( tabxy 
( tabxy 
THE A 



} 



UPDATES STATUS OF A U V OPERATING PARAMETERS 
wrlteln (tabxy (xl+37,yl+6),updatedepth:6:3); 

(xl+37,yl+7),updatepitch:6:3); 
(xl+37,yll8),updatepitchrate:6:3); 
(xl^37,ylf9),updatedivevolt:6:3); 
(xl+37,yl+10),biasl:8:6); 

U V CONTROL STATUS 
write (tabxy (xl+30,yl+13),updatetargetdepth:6:2); 
case updatemode of 

run : writeln (tabxy ( xl 4 30 , yl +1 4 ) , ’ RUN *); 
reset: writeln (tabxy ( xl 4 30 , yl 4 1 4 ) , * RESET * ) ; 
exit : writeln (tabxy ( xl 4 30, yl 414 ) , ’ EXIT »); 
end ; 

case upda teatt 1 tude of 

maintain : writeln (tabxy ( xl+30, yl 4 1 5 ) , ’ MAINTAINI NG DEPTH ’); 

climb : writeln (tabxy ( xl 4 30 , yl 4 15 ) , » CLIMBING TO TARGET DEPTH’); 

diving : writeln (tabxy ( xl 4 30 , yl 41 5 ) , ’ DI VING TO TARGET DEPTH ’); 

end ; 



end ; 



{ 



-UpdateRunModeScr een- 



procedure Ge tDig i ta iSensor yData ( var depthanaloqvo 1 t s , p I tcha na logvolts , 

pitchrateana logvol ts rreal) ; 

{ Tills procedure uses PCLAB routines to sample selected input telemetry 
channels from the AUV and digitizes these inputs and multiplies them 
by the specified gains. 



DT 2801-A / DT 707 Board set up: 



channel 1 - AUV depth input 

channel 2 - AUV pitch Input 

channel 3 - AUV pitchrate input 
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const 



{ These are AUV to DT 2801-A / DT 707 hook up board channel configurations, 

conversion and computational arguments. ) 

depthchannel = 1; { AUV output to DT-707 input channel assignment } 

depthpfs = +10.0; { Peak depth signal value } 

depthmfs = “10.0; { Minimum depth signal value } 

pitchchannel = 2; { AUV output to DT-707 input channel assignment } 

spdpfs = +10.0; { Peak pitch signal value } 

spdmfs = -10.0; { Minimum speed signal value } 

pi tchr atechannel = 3; { AUV output to DT-707 input channel assignment } 

pltchratepfs = +10.0; { Peak pitchrate signal value } 

pitchratemfs = -10.0; { Minimum pitchrate signal value } 

noc = 4096; { Number of Codes; conversion resolution. 

The DT 2801-A performs a 12 bit conversion. 

NOC = { 2 conversion bits), le 4096 } 



{ SetUpAdc and ADConTrigger PCL function arguments 

: p 6-8 PCL documentation } 



boardnum =1; 

numa2dchan = 3; 
tlmingsource = 2; 
adcgalnl = 1; 

adcgain2 = 2; 

adcgain4 = 4; 

) 

adcgain8 = 8; 

startchannel = 1; 
endchannel = 3; 

var 

pi tchadv, 
depthad V, 
p i tchr a teadv, 

3 igna ladv, 
counter, status, 
chanum, i , j 



{ -- Sets a external tr igger , internal 
{ Sets the A/D gain; 1,2, 4,8 



clock } 
are options 



Integer ; 



{ Signal analog data value } 



begin 



procedure GetDlgltalTelemetry 



} 



{ Set up the DT 2801-A board to take data. ) 

status := SelectBoard (boardnum); 

{ Set up the DT 2801-A board to take data from 3 Input channels; Data sampling 
is initiated by the ADConTrigger single channel sample of the depth channel 
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and then single ADC value samples of the speed and pJtchrate follow. 

The Trigger is connected to the DT 707 board at terminal 49 from a signal 
generating source. 

status := ADConTrigger ( depthchanne 1 , adcgainl^ depthadv ); 
status := ADCValue { p i tchctia nne 1 , adcgainl, pitchadv ); 
status ADCValue ( p i tchra techanne 1 , adcgainl, pitchrateadv ); 



{ Convert the digitized Analog Data Values for pitch, depth, pitchrate to 
analog voltage values. The algorithm for this conversion is found in 
Appendix D of the PCLAB documentation. ) 



depthana log VO 1 ts := ( depthadv * ( depthpfs-depthmf s ) /noc 
pi tchanalogvolts := ( pitchadv * ( spdpf s~spdmf s ) /noc ) 



+ depthmfs; 
+ spdmfs; 



p i tchrateana logvol ts := ( pitchrateadv * 

( pi tchr a tepf s - pi tchratemf s ) /noc ) 

+ pi tchratemf s ; 



{ status := Terminate; ) I PCL routine, closing the DT 2801-A 

end; { procedure GetDigi ta ITe lemetry ) 

procedure ATTITUDE^ ( tdepthvolts, adepthvolts : real; 

var attitude : auvattitude ); 



const 

depthcontr ol toler ence = 0.1; 

modelgain = 1.0; 



var 



{ This simulates a model referenced ) 

I gain parameter ) 



voltsdif f erence : real; 



begin { Errorvolts 

voltsd I ff erence := tdepthvolts - adepthvolts; 



Control voltage filter 



{ These conditions check if depth is within tolerence. } 



if ( voltsd i f f erence > 0) and 

( abs ( voltsd i f f erence ) <= depthcontr ol tolerence ) then 

begin 

attitude := maintain; 



end 

else if ( vol t sd i f f e r ence < 0) and 

( abs ( vol tsd I f f er ence ) <= depthcon t r o 1 to le r ence ) then 

beg i n 

attitude := maintain; 

e nd 

{ This condition checks if actual depth is less than target 4 tolerence.) 

else if ( vol tsd i f fer ence > 0) and 

( abs ( voltsd i f f er cnce ) > depthcontr o I to ler ence ) then 

beg I n 

attitude := diving; 

e nd 

( This last condition checks to see if the actual depth is more than target + 
tolerence . ) 

else if ( voltsdifference < 0) and 

( abs ( vol tsd 1 f fer ence ) > depthcontr oltoler ence ) then 



beg 1 n 

attitude := climb; 
end; 



end; ( Errorvolts 



) 



procedure EST(var up, yq , z : r ea 1 ; va r vha t , pi tch : r ea 1 ; var initial: integer; 

var bias,al,a2,a3: real); 

(This procedure uses a Model Based Compensator to determine pitch angle. 
A parameter estimation using RLS fit determines sensor bias.) 

label 3; 

type 

try = array(1..10) of real; 
trytry = ar ray ( 1 . . 10 , 1 . . 10 ) of real; 
try40 = ar ray ( 1 . . 40, 1 . . 40 ) of real; 
tryt40 = array! 1 .. 40 ) of real; 

const 

ns = 1; 



va r 

beta,g,khat, xda ta,g2,phid,phiq, fc,phl2,khat2, xha t2 , xha t , 
g3,h3,khat3,phi,phat,xu,xy,xz,xs,xnev: try; 
spt, f ,spt2, f 3,spt3 : trytry; 
nx,np,ny,nl,nf,n2,nsnf,time,i,j : integer; 

2d,zf,zf2,uf,yf,fs,af, y, yhat , ud,u,yd,p0,sr3,sr2,sr,sq3,sq,sq2, 
zdd , sg2, s Igmax, gbias: real; 
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procedure Initialize Arrays; 
begin 

for 1 := 1 to 10 do 

begin 

beta ( 1 ] : =0 . 0; 
khat ( 1 ] : =0.0; 
xha t ( 1 ] : = 0.0; 

phi( 1 ] := 0.0; 

XU ( 1 ] : =0 . 0 ; 
xy ( 1 1 : =0 . 0 ; 
xzj 1 ] ; =0 . 0 ; 
xnev ( 1 ] : =0 . 0 ; 

end ; 

np := 2* ns; 
nx := np + 1; 
ny := 1; 
y:= 0.0; 
uf ; = 0.0; 
ud : =0 . 0; 
yd : = 0 . 0 ; 
yf 0.0; 
zd : = 0.0; 
z f ; = 0.0; 
time := 0; 

{slgmax = variance of the bias] 

sigmax ;= 0.1; 

sq := slgmax; 

sq2:= slgmax; 

sq3 := slgmax; 

sr : = 1.0; 

sr 2 : = 1.0; 

sr 3 : = 1.0; 

pO := l.OeS; 

for 1 : = 1 to nx do 
begin 

for j;= 1 to nx do 
begin 

spt I 1 , j ] : = 0.0; 
spt2( 1, j ] := 0.0; 
fli,j) := 0.0; 
f 3tl, j 1 !=0.0; 

if 1 = j then 
begin 

sptl 1^ jl := pO; 
spt2 ( 1 , j ] : =p0; 
spt3( 1, j ] :=p0; 
ftl,jl:= 1.0; 
end ; 

end ; 

gll]:= 0.0; 
g2tll:= 0.0; 



end ; 
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g 1 nx ) : 


= 


1.0; 


g2l 2) : 


= 


1.0; 


{ fell) 


: = 


2 .9272E-2; 


fcl 2 ) ! 


= 


-6 .9013E-1; 


£cl 3 ) : 


= 


1 . 3633E-1; 


£ c 1 4 ) ; 


= 


-7 . 5566E-2; 


£cl5) : 


= 


0.0; 


£c(6) : 


= 


9 . 8861E-2; 


£cl7) : 




3.9545E-1; 


£c(8) 




5.9317E-1; 


£cl9 1 : 


= 


3 .9545E-1; 


1 fcllO) 


: = 


9 . 8861E-2; } 



fcll]:=0. 80000; 
fct2 1 :=0. 20000; 
nf:= 1; 
fs := 5.0; 

f3(l,l):=0.6977; {discretized A-ma tr ix ) 

E3( 2, 1 ] : =0. 1G80; 

£3(2,21 :=1.0; 

£3(3,11 :=-0.017B; 

£3( 3, 2 1 ;=-0.20; 

£3(3,31:=1.0; 

g3(l):=0.4251; {discretized B-matr ixl 

g3(21 !=0.0451; 
g3( 3 1 :=-0. 0031; 

{ khat3(ll:=-0,0073;khat3(21:=-0.1046;khat3(31:=0.4250;l 

{gains £or 5Hz . , poles0 { 0 . 7 5 0.76 0.771 kob7 1 
khat3(l 1 :=0,0;khat3( 2 1 : =-0 . 4 029 ; kha 1 3 ( 3 1 : =0.567 7; 

{kob8 @ ob8poles={.7 .71 .721 at l£t/s) 
b i a 5 : = 0 . 0 ; 

end ; 



procedure innerprod ( var y9 : real;var phi9, x9 : try; var nx9 : integer 1 ; 



var 

j : integer ; 

begin 
y9:= 0.0; 

for j := 1 to nx9 DO 
begin 

y9 := phi9(jl ♦ x9(Jl+ y9; 

end ; 
end ; 

procedure update(var phi8: try;var u8,y8: real; ns : integer 1 ; 



var 

n2 : integer ; 
i ; integer ; 
phisave: try; 
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beg I n 



n2 : =ns* 2 + 1 ; 
for i:=l to n2 do 
beg i n 

ph isa ve ( i ] : =ph i 8 ( i 1 ; 
end ; 



i : = ns ; 

while i >= 2 do 
begin * 

phi8(i]:= phisavell - 1]; 
phl8(ns + i) := ph isave ( ns + i - 1 ] 
i := i - 1; 



end ; 

phi8ll]:= y8; 
phi8(ns + 1) := u8; 

end ; 



procedure filter(var u4 : real ; var x4 , f c4 : try ; var nf 4 : integer ) ; 
var 

nf42 : integer; 
savex4 : try ; 

begin 

update(x4,u4,y4,nf4); 
nf42:= 2 * nf4; 

5nnerprod(y4,x4,fc4,nf42); 

{vriteln(* inside filter after inner,y4,x4=\y4,x4(l],x4(21,x4l 

3)); 

) 

end; 



procedure nevest(var nx7: integer;var 

var y7:real); 



var 



delta : real ; 
i , j : integer ; 

xsave : tr y ; 



xhat7 , khat7 , h7 : 



try; 



begin 

for i : =1 to nx7 do 
begin 

xsave ( i ] : =xhat7 ( i ] ; 
end ; 

delta := y7; 

for j:= 1 to nx7 do 
begin 

delta:= delta - h7lj) ♦ xhat7(jl; 
end ; 

for 1 : = 1 to nx7 do 
begin 

xhat7ti] := xsave(i] + khat7(i] ♦ delta; 
end ; 

end ; 
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procedure hholder(var nr6,nc6; integer;var ml6: try40); 
va r 

l,col,i,j,k: integer; 

temp, s I gma , s urn : real; 
d6: tryt40; larray(1..40) of real;} 
c6, h6 : try40; { ar ray 1 1 . . 40, 1 . . 40 I of real; 1 



beg i n 


for col:= 1 to nc6 do 


beg 1 n 


sum := 0.0; 

for j := col to nr6 do 
beg i n 

sum:= sum + ml 6 ( j , co 1 ) *ml 6 1 j , co 1 ] ; 


d 6 1 k ) ) /sum ; 


end ; 

s igma := sqrt(sum); 
for j : = col to nr 6 do 
begin 

d 6 t j 1 : = 0.0; 
end ; 

d 6 t CO 1 ) : = s Igma ; 

sum := 0.0; 

for j : = col to nr 6 do 

begin 

sum := sum + sqr ( ml6 [ j , col ) -d6 ( j ) ) ; 
end ; 

for j:= col to nr6 do 
begin 

for k:= col to nr6 do 
begin 

beg 1 n 

Jf abs(sum) < 0.00001 
then temp:= 0.0 

else temp := -2* (ml6 ( j , col ) -d6 ( j ) ) * (ml6 ( k , col 1 
end ; 

if j = k then 
begin 

temp:= 1.0 + temp; 
end ; 

h6 1 j , k ) : = temp; 
end ; 

end ; 

for j:= col to nr6 do 
begin 

for k:= col to nc6 do 
begin 

temp : = 0 . 0 ; 

for 1:= col to nr6 do 

begin 

temp:= temp + h6 1 J , 1 ) *ml6 ( 1 , k ) ; 

end ; 



67 



end ; 



c6 ( j ^ k 1 : = temp; 



end ; 

I ml6 : = c6 ; } 



for l:=col to nr6 do 
begin 

for j:=col to nc6 do 
beg I n 

ml6ll, j) :=c6(i, jl; 

end; 

end ; 

end ; 

for i : = 2 to nr 6 do 
beg i n 

f or j : = 1 to i-1 do 
beg i n 

ml6[i, j] := 0.0; 

end ; 

end ; 

end ; 

procedure kgain(var nx5: integer;var spt5,f5: trytry; 

var g5, h5 : try; var sr 5, sq5 : rea 1 ; var khat5: try); 

var 

ny/ i / j / nu ^ k , nytot , nxSnu : integer ; 

ml5 : try40; 
sptt : trytry; 
beg 1 n 

ml5(l,l) sr5; 
ny:= 1 ; 
nu:= 1; 

for 1;= 1 to nx5 do 
beg i n 

for j : = 1 to nx5 do 
begin 

ml5 ( ny+ i ^ ny+ j 1 : = spt5(j,i); 

end ; 

end ; 

for i : = 1 to nx5 do 
beg i n 

ml5 ( ny+ i , 1 ) ; = 0.0; 
for k:= 1 to nx5 do 
begin 

ml5(ny+i,l) := ml5 ( ny + i , 1 1 +spt 5 1 k , i )*h5(k); 

end ; 

end ; 

for j:= ny+1 to ny+nx5 do 
beg in 

ml5[l, j] := 0.0; 

end ; 

nytot;= ny+nx5; 

{ writeln (’enter hholder first time’); } 
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{ wrltelnl 1 , ml5 1 1 , 2 ) , ml 5 ( 1 , 3 ] ) ; ) 

hholder(nytot,nytot,ml5); 

( wr 1 teln( ’ ml5af ter hholder frst * ^ ml 5 1 1 ^ 1 ) , ml 5 ( 1 , 2 ) , ml5 ( 1 , 3 J ) ; ] 

if abs ( ml 6 [ 1 , 1 ) ) <= 0.0000001 then vriteln(’R is singular*); 
for i:= 1 to nx5 do 
beg 1 n 

khat5tl]:= ml5 t 1 , i + 1 1 /ml5 t 1 , 1 ) ; 

( wr lteln( » khat5 * , khat 5 1 i ] , * ’ / i ); ) 

for j : = 1 to nx5 do 
beg 1 n 

sptttj,i):= ml5tny+i, ny+ j ) ; 

end ; 

end ; 

for 1:= 1 to nx5 do 
begin 

for j : = 1 to nx5 do 
begin 

ml5li, jl := 0.0; 

for k:= 1 to nx5 do 

begin 

ml5( 1, j 1 ml5( i, j )+sptt(k, 1 )*f 5( j,k ) ; 

end; 

end ; 

end; 

for j : = 1 to nx5 do 
begin 



ml5tnx6+l,j]:= sq5*g5tj); 

end ; 

nx5nu := nx5+nu; 

( wr iteln( *enter hholder second time*); ) 
hholder ( nxSnu , nx5^ml5 ) ; 
for i : = 1 to nx5 do 
begin 

for j : = 1 to nx5 do 
begin 

sptSt j,i) := mlSti, j); 

end ; 

end; 

end; 



begin *beg inning of estimater ) 

{Wrlteln( *tlme=*,time, *initial=*^initlal);} 
if initial = 1 
then 

begin 

initialize Arrays; 
goto 3; 

end ; 
ud : =uf ; 

filter ( up, uf, XU, fc,nf); 
yd := yf; 
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{ 

for j:= 1 
[********* 



f 1 1 ter (yq, yf , xy, fc, nf ) ; 
zdd ; = zd ; 
zd := zf; 

£llter(z,zf,xz, £c,n£l ; 
update ( ph 1 , ud , yd , ns ) ; 
ph 1 t nx ) ; . 0 ; 
nsn£ := ns + nf; 
tlme:=tlme+l; 

If time <= nsnf then goto 3; 

***** estimate bias ***** ) 

kgain(nx,spt/f,g,phl ,sr,sq,khat) ; 

{ wrlte(’phl and yf into ne ves t ’ , pii 1 1 1 ) , ph 1 t 2 ) , ph 1 1 3 ) , y f ) ; 1 
{ wrlte( ’khat ^khattll,khatt21,khatl3n; 1 

newes t ( nx, xha t , kha t , ph 1 , y f 1 ; 

{ vriteln( 'paras-’, 

xhat 1 1 ) , xhat 12), xha 1 1 3 1 , xhat I 4 1 , xhat 151) ; 1 
{*** compute bias estimate ***) 
i : = ns ; 

while 1 >= 2 do 
begin 

betat 1 ) := betat 1 -11; 

1 : = 1 - 1 ; 

end ; 

beta 1 1 1 : = bias ; 
blas:= xhatlnxj; 

Wrlteln( 'blas=*,blas);l 
to ns do 
begin 

blas:= bias 4 xha 1 1 j ) *beta t j ) ; 

end ; 

al : =xha 1 1 1 1 ; a2 : =xhat I 2 1 ; a3 : =xhat I 31;) 



estimate ********* 
phl2tll := yf; 
phl2{2) ;= 1,0; 

zf2 := (zf - 2.0*zd 4 zdd 1 *sqr ( f s ) ; 
n2 := 2; 

kgaln(n2,spt2,f,g2,phl2,sr2,sq2,kiiat2); 

neves t ( n2 , xha 1 2 ^khat2,phl2,zf2); 

If abs(xhat2lin > 0.00000001 
then gblas:= xhat 2 t 2 l/xhat2 [ 1 1 ; 
vhat : = xhat 2 111; 

*** estimate pitch angle *** 

f3(l,l):= 1.0; 
f3(l,2):= -1/fs; 
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f 3(2,1);= 0.0; 
f3l2,21 := 1.0; 
g3(l):= 1.0; 
g3(2) := 1.0; 
h3(l] := 1.0; 
h3(2) := 0.0; 

kgain(n2,spt3,f3,g3,h3,sr3,sq3,khat3); ) 

{observer for plytcli ajh, 10/2/881 
begin 

for i:=l to 3 do 
beg i n 

xnev ( I ] ; =0 . 0 ; 
for j : =1 to 3 do 

xnew(i):=xnevli)+f3(l,jl*xs(j]+g3(i)*up/10.0+khat3(l)^(z“xs(31+yq-xs(l 
end ; 

end;pltch:=xnev( 2] ; 
xs ( 1 1 ; =xnev ( 1 ) ; 
xs ( 2 1 : =xnev ( 2 1 ; 
xs ( 3 1 ; =xnev ( 3 1 ; 

al:=xs(l];a2:=xs(2];a3:=xs(3); 



3 rend ; 



procedure GenerateDi veplaneCoirunand ( var auvdepth, auvpi tch, auvpi tchrate , 
auvdepthcom, de 1 ta : real;var krinteger ); 

{ This procedure takes digitized voltage values of depth , pi tch, pi tchr ate , 
and target depth in volys and sends new commands for controls } 

const 

I DT 2801-A DIGITAL TO ANALOG Conversion declarations ) 

d2achannel0 = 0; 
pfs = 10.0; 
mfs = - 10.0; 
noc = 4096; 

T=0 . 2 ; amp=0 . 05; T0=2 . 0 ; {these parameters include sin excitation) 

{ gI=0.3066;g2=0.8604;g3=-0.2493;) 

{these gains are for 20 Hz.{? poles=(. 92 .925 .926 )) 

{ gl=-0 .2286; g2=0 .1944; g3=-0. 0118; 

these gains are for 5Hz. and poles & (0.92 .925 .926)) 

gl= 0.2060;g2=0.6573;g3=-0.1605; {for5Hz. at (0..85 0.860 0.870)) 



var 

d igi taldatavalueO, status : integer; 

e 1 , e 2 , e 3 : rea 1 ; 

function ConvertAnalog2Digi tal ( analogvalue : real ): integer; 
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{ This function converts analog signal volts to an equivalent 
digital value. See App D of PCLAB book, ) 

var 

lemp : real; 

beg in 

temp := ( analogvalue - mfs ) ♦ ( (noc - 10) / (pfs ~ mfs ) ); 

convertanalog2digital := round ( temp ); 

end ; 

begin { Genera teD 1 veP laneCommand ) 

e 3 : = ( auvdepthcom-auvdepth ) ; 
e 2 : = ( a uvp i tch ) ; 
el:=(auvpitchrate); 

if abs(e3)<= 0.000001 then e3:=0.0; 
if abs(elX = 0. 000005 then el:=0,0; 
if abs(e2) <=0.000005 then e2:=0,0; 

delta :=g3*e3 f gl*(-el) fg2*(-e2); 

if(abs(delta)>0.1) then 
begin 

delta :=0 .l*abs(delta)/delta; 
end ; 

delta := 10*delta; 

digi taldatavalueO := convertanalog2d igl tal ( delta ); 
status := dacvalue ( d2achannel0, d igitaldatavalueO ); 

{ status := terminate; ) 

end; { Genera teDiveP laneCommand ) 

procedure MoveKvar r0,u,r :real); 
const 

countmax=400 ; 
count 2=200 ; 

{type 

act ive controlmode = (run, reset, exit); 
auvattitude = ( cl imb, ma inta in, d i v ing ) ; ) 



var 

count : integer ; 
dr,xl,x2,x3 :real ; 
act i ve mode t act iveCONTROLmode ; 
attitude :auvattitude; 

begin 

act i vemode ; = reset; 
attitude : =cl imb; 
count:=0 ; 



72 



while (count<count max) do 

begin 

dr :=0.010; 

Ge to I g 1 talSensor yDa ta ( xl , x2 , x3 ) ; 
lf(count>count2)then dr:=0.0; 
r :=r04dr*count; 

{ I f ( count >count 2 ) then dr:=0.0;) 

Genera teD ivep] a neCommand (xl,x2,x3,r,U/COunt); 
count:=count^-l; 

{ UpdateRunModeScreen(x3,x2,xl,r , act 1 vemode , a tt i tude ) } 

end ; 

end ; 



procedure I n i t la 1 1 zePar ame t er s ; 

( This procedure Initializes all declared control and display parameters to 
zero. 1 

begin { procedure Initial IzeParameters 1 

auvpltchvol ts :=0.0 ; 
auvdepthvol ts := 0.0; 
auvspeedvolts := 0.0; 
auvpl tchratevol ts 0.0; 
auvdepth := 0.0; 
auvspeed := 0.0; 
auvpltchrate := 0.0; 
es tdeptli : =0 . 0 ; 
err : =0 . 0; 

end; ( procedure Ini t lallzeParameters ) 



begin { Act 1 veContr o 1 } 



In 1 1 lal 1 ze parameter s ; 

Initial := 1; { In 1 1 la 1 Ize Ar rays In EST procedure) 

5: 

time := 1; {Initializes beginning of data file) 

c 1 r scr ; 

vr lteln( *DATA FILE NAME? lie f082201.dat)*); 
reading fl 1 e name ) ; 
asslgnlfllevar, filename); 
r evr 1 te ( f 1 levar ) ; 
clr scr ; 

activemode := run; 

repeat { Repeat until activemode = exit ) 



{ ClosedLoopContr olscreen; ) 

GetTar getDepth ( targetdepth, tar getdepthvolts ); 
RunModeScreen ; 
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1 : 

while ( not keyhit ( modereply, modereply2)) do 
beg I n 

upda tecounter := 0; 

while ( upda tecoun te r < upda te 1 ncr emen t ) do 
beg i n 

GetDigltalSensoryData (auvdepth volts, auvpi tch vol tsxx,auvpitchratevolts) ; 



Genera teDlveplaneCommand ( ac3,ac2,acl, 

targeldcpthvolts,divevolts,tlme ); 

EST (dive volts, auvpitch rate VO lts,auvdepthvolts, 

speed,auvpi tch volts, Inltial,blasl,acl,ac2,ac3) ; 
writeln(filevar,time:5,auvdepthvolts:12:6,auvpltchvolt3xx:12:6, 
auvpitchratevolts:12:6,auvpitchvolts:8:3,divevolts:12:6, 
blasl:12:6,acl:12:6,ac2:12:6,ac3:12:6); 



ATTI TUDE_ ( ta r ge tdepthvol ts , auvdepth volts, attitude) ; 
initial 0; 

updatecounter := updatecounter + 1; 
time := time + 1; 

end; { while updatecounter < upda te increment ) 



UpdateRunModeScreen ( auvdepthvolts, auvpitchvoltsxx, 

au vp itch rate volts, dive volts, targe tdepth, active mode, attitude,biasl); 

end; { while not KeyHit ) 

if ( ord (modereply ) = 27) and ( or d ( modereply2 ) = 59) then { KeyHlt=Fl ) 
begin 

close ( f ilevar ) ; 

goto 5; 

end 

else if ( ord ( modereply ) = 27) and ( ord ( moder eply2 ) = 60) then 
begin 

activemode := reset; 

Move 1 ( tar ge tdepth VO Its , dive volts, tgtnew) ; 
tar getdepthvol ts : =tgtnew ; 

GOTO 1 

end 

else if ( ord ( moder eply ) = 27) and ( ord ( moder eply2 ) =61) then 
begin 

close ( f Ilevar ) ; 
activemode := exit; 

end { KeyHlt= f3) 

until (active mode=exit) 



end; ( Act i veCon tr ol J 
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begin ( S ta t us AndCommand } 

repeat 

S ta t us And Comma ndSct een ; 

GetKey { * ' , mode , mode 2 ) ; 

if { ord (mode) = 27 ) and ( ord (mode2) = 59 ) then 
begin 

clrbox2 (xl,yl,x2^y2); 

ClosedLoopControl ; 

end ; 

until ( mode in ); 

end; { S ta tusAndCommand ) 



pr oced ure InitializeZeroDigitalSignalOut; 

{ This procedure MUST be executed as the first procedure called in the main 
program to Insure a zero signal out on the 2 output channels. Otherwise the 
DT 2801-A board defaults to a minimum full scale output. } 

cons t 



digitalchanO = 0; 

dlgltalchanl = 1; 

digi talcommandboard = 1; 

var 

status, 

d igi taldatavalue : integer; 



beg in 

d Igl ta Idata va lue := 2040; { This will be converted to an equivalent 

zero analog signal out on a 12 bit 
resolution converter like DT 2001-A. } 



status 
status 
status 
status 
status 
end ; 



initialize; 

selectboard ( d igl ta Icommandboard ); 
dacvalue ( d Igl talchanO, d I g I ta Ida tava lue ); 
dacvalue ( d ig i ta Ichanl , d ig i talda ta va 1 ue ); 
terminate; 



procedure Deact i vateADBoardAndExi tPr ogram; 

{ This procedure deactivates the DT 2001-A board and presents an exit screen.) 



var 
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s ta tus 



: integer; 



Deact 1 vateADBoardAndExi tProgram 



begin { 

status := terminate; 
clrbox2 (xl,yl,x2,y2); 

boxprlnt ( yl+10 , x 1 , x 2 , » THI S CONCLUDES YOUR AUV AUTOP ILOTTI NG SESSION , BYE*); 



end; { 



Deact i vate ADBoardAndEx ItProgram 



BEGIN 



{ 



MAIN PROGRAM 



Initial! zeZeroDlgltalSigna lout; 

clr scr ; 

repeat 

Main Menu ( option ); 

if ( option in [*R*,*r*l) then 
begin 

repeat 

begin 

S tatusAndCommand ( controlmode ); 
end ; 

until ( controlmode in l*q*, *□*)); 
end ; 

until ( option in [*Q*,*q*l); 



Deact iva te ADBoardAndEx i tPr ogram; 
END. { 



MAIN PROGRAM 
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